#include <chrono>
#include <memory>
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32_multi_array.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "../../e_wheel/include/data_type_define.hpp"
#include "jsoncpp/json/json.h"
using namespace std::chrono_literals;
using std::placeholders::_1;
/* This example creates a subclass of Node and uses std::bind() to register a
 * member function as a callback from the timer. */

class Yaokong_Msg : public rclcpp::Node
{
public:
  Yaokong_Msg()
  : Node("yaokong"), count_(0)
  {
    publisher_ = this->create_publisher<std_msgs::msg::Int32MultiArray>("yaokong", 10);
    timer_ = this->create_wall_timer(
      10ms, std::bind(&Yaokong_Msg::timer_callback, this));
    subscription_ = this->create_subscription<std_msgs::msg::Int32MultiArray>(
      "e_wheel", 10, std::bind(&Yaokong_Msg::topic_callback, this, _1));

      dc = new Data_Class();
    // sub_nav_ = this->create_subscription<std_msgs::msg::String>(
    //     "/nav_cmd", 10, std::bind(&Yaokong_Msg::nav_callback, this, _1)); 
    twist_sub_  = this->create_subscription<geometry_msgs::msg::Twist>(
      "/nav_cmd", 10, std::bind(&Yaokong_Msg::twist_callback, this, _1)); 
    yaokong_nav_sw=0;    
    yaokong_count=0;
  }

private:
 
  void timer_callback()
  {
     auto message = std_msgs::msg::Int32MultiArray();
     message.data.resize(40);
    if(yaokong_count>0)
     {
      yaokong_count--;
      return ;
     }
   //  dc->yaokong();
   float linex,wg,liney;
   linex=dc->convert_yaogan_line(dc->yaogan[1]);
   wg= dc->convert_yaogan_wmga(dc->yaogan[2]);
   liney=0;
   if((wg<0.01)&&(wg>-0.01))liney = dc->convert_yaogan_line(-dc->yaogan[0]) * 0.6;
   dc->yaokong_real(linex,wg,liney);

     dc->data_control_to_msg(message.data);
 // RCLCPP_INFO(this->get_logger(), "Publishing:speedset= '%d' '%d' '%d' '%d'", dc->speedset[0],dc->speedset[1],dc->speedset[2],dc->speedset[3]);
 //   RCLCPP_INFO(this->get_logger(), "Publishing:anglecur= '%d' '%d' '%d' '%d'", dc->anglecur[0],dc->anglecur[1],dc->anglecur[2],dc->anglecur[3]);
 //  RCLCPP_INFO(this->get_logger(), "Publishing:anglen= '%f' '%f' '%f' '%f'", dc->angle_n[0],dc->angle_n[1],dc->angle_n[2],dc->angle_n[3]);
  //
 //  RCLCPP_INFO(this->get_logger(), "Publishing:angleset= '%d' '%d' '%d' '%d'", dc->angleset[0],dc->angleset[1],dc->angleset[2],dc->angleset[3]);
     publisher_->publish(message);
  }
  void twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg) 
  {
        double linex,wg,liney;
              linex = msg->linear.x;
              liney = msg->linear.y;
              wg = msg->angular.z;
              yaokong_count=100;
              //导航控制
             // if((wg<0.01)&&(wg>-0.01))liney = dc->convert_yaogan_line(-dc->yaogan[0]) * 0.6;
              dc->yaokong_real(linex,wg,liney);
              auto message = std_msgs::msg::Int32MultiArray();
              message.data.resize(40);
              dc->data_control_to_msg(message.data);
              publisher_->publish(message);
            
      
    
  
    
  }
  void nav_callback(const std_msgs::msg::String::SharedPtr msg) 
  {
    try
    {
      
        Json::Reader reader;
        Json::Value json_val;
        float linex,wg,liney;
       
        if(reader.parse(msg->data, json_val))
            {
               yaokong_count=100;
              linex = json_val["act"]["x"].asDouble();
              liney = json_val["act"]["y"].asDouble();
              wg    = json_val["act"]["w"].asDouble();
              RCLCPP_INFO(get_logger(), "pose: %.3f, %.3f, %.3f", linex, liney, wg);

              //导航控制
             // if((wg<0.01)&&(wg>-0.01))liney = dc->convert_yaogan_line(-dc->yaogan[0]) * 0.6;
              dc->yaokong_real(linex,wg,liney);
              auto message = std_msgs::msg::Int32MultiArray();
              message.data.resize(40);
              dc->data_control_to_msg(message.data);
              publisher_->publish(message);
            }
      
    }
    catch(const std::exception& e)
    {
      std::cerr << e.what() << '\n';
    }
    
  }
   void topic_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) const
    {
      dc->data_msg_to_indata(msg->data);
      dc->Feedback_cal();
      // RCLCPP_INFO(this->get_logger(),
      //      "Publishing:speedcur= '%d' '%d' '%d' '%d' '%d' '%d' '%d' '%d'",
      //      dc->speedcur[0],dc->speedcur[1],dc->speedcur[2],dc->speedcur[3],
      //      dc->speedcur[4],dc->speedcur[5],dc->speedcur[6],dc->speedcur[7]
      //      );
           float spf=dc->convert_rmp_to_ms(dc->speedcur[0]);
      // RCLCPP_INFO(this->get_logger(),
      //       "realline = '%f ' vx ='%f' vy= '%f' wz= '%f' ",
      //      spf,dc->cur_vx,dc->cur_vy,dc->cur_wz);    
      // RCLCPP_INFO(this->get_logger(), "Publishing:anglecur=LUN_L_B '%d'LUN_L_F '%d' LUN_R_F'%d'LUN_R_B '%d'", 
      //       dc->anglecur[LUN_L_B],dc->anglecur[LUN_L_F],dc->anglecur[LUN_R_F],dc->anglecur[LUN_R_B]);

    //  RCLCPP_INFO(this->get_logger(), "yaokong heard: '%x' '%x' '%x' '%x'",dc->yaogan[0],dc->yaogan[1],dc->yaogan[2],dc->yaogan[3]);
    }
   rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr subscription_;
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr publisher_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_nav_;
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
  size_t count_;
  int32_t cctt=10;
  int32_t wheel_out_msg[50],wheel_in_msg[50];
  Data_Class *dc;
  int yaokong_nav_sw;
  int yaokong_count;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<Yaokong_Msg>());
  rclcpp::shutdown();
  return 0;
}